%author: wxj233
%time: 2023.11.10 10:00
%function:  轨迹


classdef Track < handle
    %TRACK 此处显示有关此类的摘要
    %   此处显示详细说明
    
    properties
        id  % 轨迹id
        fps  % feature points特征点[fp1, fp2, ...]单个fp包含{points, Xk_e, Pk_ee, fplife}
        tlifeMax  % 轨迹最大生命值

        validFpNum  % 本次跟新有效量测数量
        tlife  % track life,轨迹当前生命值，每记忆一次减一
        mNum  % 量测数量(measure Number),有效量测次数,每有效关联一次加一,轨迹初始时为0，也就是初始的点不计算进来，也不保存
        len  % 轨迹总长度,每迭代一次加一，轨迹初始时为0，也就是初始的点不计算进来，也不保存
        gXk_e  % 群状态[x1, vx1, y, vy1, x2, vx2, y2, vy2, ...]'
        gPk_ee  % 群状态估计误差协方差矩阵
        mPcov  % 中心点状态，协方差矩阵

        points  % 群状态记录[t, x, vx, y, vy; ...]，位置取均值
        mfpTracer  % 该轨迹所属追踪器
    end
    
    methods
        function obj = Track(tlifeMax, mfpTracer)
            %TRACK 构造此类的实例
            % tlifeMax: 轨迹最大生命值
            obj.id = obj.getId();  % 自动递增id
            obj.fps = struct([]);
            obj.tlifeMax = tlifeMax;
            obj.mNum = 0;
            obj.len = 0;
            obj.tlife = obj.tlifeMax;
            obj.validFpNum = 0;
            obj.points = [];
            obj.mfpTracer = mfpTracer;
            obj.gXk_e = [];
            obj.gPk_ee = [];
        end
        

        function vfusion(obj, p)
            % 速度融合
            % p: 群相关性因子，取0~1，0代表不相关，1代表强相关
            % 完成后更新群状态，群协方差矩阵，更新特征点的状态以及协方差矩阵
            fpNum = length(obj.fps);
            F = zeros([fpNum*4, fpNum*4]);  % 融合矩阵
            if obj.validFpNum > 0  % 有有效量测关联时
                for i = 1:1:fpNum
                    if obj.fps(i).isValidAss
                        F((i-1)*4+1, (i-1)*4+1) = 1;  % x
                        F(2:4:fpNum*4, (i-1)*4+2) = p/obj.validFpNum;
                        F((i-1)*4+2, (i-1)*4+2) = 1-p+p/obj.validFpNum; % vx
    
                        F((i-1)*4+3, (i-1)*4+3) = 1;  % y
                        F(4:4:fpNum*4, (i-1)*4+4) = p/obj.validFpNum;
                        F((i-1)*4+4, (i-1)*4+4) = 1-p+p/obj.validFpNum; % vy
                    else
                        F((i-1)*4+1, (i-1)*4+1) = 1;  % x
                        F((i-1)*4+2, (i-1)*4+2) = 1-p; % vx
    
                        F((i-1)*4+3, (i-1)*4+3) = 1;  % y
                        F((i-1)*4+4, (i-1)*4+4) = 1-p; % vy
                    end
                end
            else  % 没有有效量测关联
                for i = 1:1:fpNum
                    F((i-1)*4+1, (i-1)*4+1) = 1;  % x
                    F(2:4:fpNum*4, (i-1)*4+2) = p/fpNum;  % 既然没有有效量测，那就每一个特征点都贡献一份力量
                    F((i-1)*4+2, (i-1)*4+2) = 1-p + p/fpNum; % vx

                    F((i-1)*4+3, (i-1)*4+3) = 1;  % y
                    F(4:4:fpNum*4, (i-1)*4+4) = p/fpNum;
                    F((i-1)*4+4, (i-1)*4+4) = 1-p+p/fpNum; % vy
                end
            end

            obj.gXk_e = F*obj.gXk_e;
            if ~all(obj.gPk_ee==obj.gPk_ee', "all")
                disp("不对称了");
            end
            obj.gPk_ee = F*obj.gPk_ee*F';
            if ~all(obj.gPk_ee==obj.gPk_ee', "all")
                disp("不对称了");
            end

            for i = 1:1:fpNum
                obj.fps(i).Xk_e = obj.gXk_e((i-1)*4+1:(i-1)*4+4);
                obj.fps(i).Pk_ee = obj.gPk_ee((i-1)*4+1:(i-1)*4+4, (i-1)*4+1:(i-1)*4+4);
            end
        end


        function predict(obj, dt, q)
            % 轨迹预测完成每个特征点的预测
            % dt: 预测时间间隔
            % q: 过程噪声量级
            fpNum = length(obj.fps);
            gA = [];
            gQ = [];
            for i = 1:1:fpNum
                [A, Q] = obj.fps(i).predict(dt, q);  % 同时更新了特征点内部状态
                gA = obj.diagcat(gA, A);
                gQ = obj.diagcat(gQ, Q);
                obj.gXk_e((i-1)*4+1:(i-1)*4+4) = obj.fps(i).Xk_e;
            end
            if ~all(obj.gPk_ee==obj.gPk_ee', "all")
                disp("不对称了");
            end
            obj.gPk_ee = gA*obj.gPk_ee*gA' + gQ;  % 更新群状态预测误差协方差矩阵
            if ~all(obj.gPk_ee==obj.gPk_ee', "all")
                disp("不对称了");
            end
        end


        function update(obj, r, nowTime)
            % 轨迹更新完成每个特征点的更新
            % r: 量测误差数量级
            % nowTime: 要更新的时刻
            obj.len = obj.len + 1;
            if obj.validFpNum > 0
                obj.mNum = obj.mNum + 1;
                obj.tlife = obj.tlifeMax;
            else
                obj.tlife = obj.tlife - 1;
                if obj.tlife == 0
                    obj.mfpTracer.deleteTrack(obj.id);
                    return;
                end
            end

            gKk = [];
            gH = [];
            gR = [];
            state = zeros([1, 5]);
            if obj.validFpNum > 0
               r = r/obj.validFpNum;
            end

            i = 0;
            for fp = obj.fps
                [Kk, H, R, isExist] = fp.update(fp.assP, r, nowTime);
                if isExist
                    gKk = obj.diagcat(gKk, Kk);
                    gH = obj.diagcat(gH, H);
                    gR = obj.diagcat(gR, R);
                    obj.gXk_e(4*i+1:4*i+4, :) = fp.Xk_e;
                    i = i + 1;
                    state = state + fp.points(end, 1:5);
                end
            end
            if ~all(obj.gPk_ee==obj.gPk_ee', "all")
                disp("不对称了");
            end
            obj.gPk_ee = obj.gPk_ee - 2*obj.gPk_ee*gH'*gKk' + gKk*(gH*obj.gPk_ee*gH' + gR)*gKk';
            obj.gPk_ee = (obj.gPk_ee+obj.gPk_ee')/2;
            if ~all(obj.gPk_ee==obj.gPk_ee', "all")
                disp("不对称了");
            end

            obj.points = [obj.points; state/length(obj.fps)]; % 新增point

            F = [];
            for i = 1:1:length(obj.fps)
                F = [F, eye(4)/length(obj.fps)];
            end
            obj.mPcov = F*obj.gPk_ee*F';  % 均值点的协方差矩阵
        end


        function fp = addFp(obj, p1, p2, r, fplifeMax)
            % 添加特征点，同时更新群状态
            % p1: t1时刻特征点[t1, x, y, id, cluster]
            % p2: t2时刻特征点[t2, x, y, id, cluster]
            % r: 测量误差方差
            % fplifeMax: 特征点最大生命值
            dt = p2(1) - p1(1);
            vx = (p2(2) - p1(2))/dt;
            vy = (p2(3) - p1(3))/dt;

            ps = [p1(1), p1(2), vx, p1(3), vy, p1(4), p1(5);
                  p2(1), p2(2), vx, p2(3), vy, p2(4), p2(5)];
            Xk_e = [p2(2), vx, p2(3), vy]';
            Pk_ee = [r, r/dt, 0, 0;
                     r/dt, 2*r/(dt*dt), 0, 0;
                     0, 0, r, r/dt;
                     0, 0, r/dt, 2*r/(dt*dt)];
            fp = FeaturePoint(fplifeMax, ps, Xk_e, Pk_ee, obj);
            obj.fps = [obj.fps, fp];

            obj.gXk_e = [obj.gXk_e; Xk_e];
            obj.gPk_ee = obj.diagcat(obj.gPk_ee, Pk_ee);

            obj.validFpNum = obj.validFpNum + 1;
        end


        function M = diagcat(~, M, m)
            % 完成子矩阵的对角线拼接
            % M: 被拼接的矩阵
            % m: 将被拼接进M的矩阵m
            lb = zeros(size(m,1), size(M, 2));
            ru = zeros(size(M,1), size(m, 2));
            M = [M,ru;lb,m];
        end


        function deleteFp(obj, id)
            % 删除指定id的特征点
            for i = 1:1:length(obj.fps)
                if obj.fps(i).id == id
                    obj.mfpTracer.deadFps = [obj.mfpTracer.deadFps, obj.fps(i)];
                    obj.fps(i) = [];
                    obj.gXk_e((i-1)*4+1:1:(i-1)*4+4, :) = [];
                    obj.gPk_ee((i-1)*4+1:1:(i-1)*4+4, :) = [];
                    obj.gPk_ee(:, (i-1)*4+1:1:(i-1)*4+4) = [];
                    break;
                end
            end
        end

    end


    methods (Static)
        function id = getId(varargin)
          % 获取特征点唯一标识
          persistent ID;
          if size(varargin, 2) == 1
                ID = varargin{1};
                return;
          end

          if isempty(ID)
             ID = 0;
          end
          ID = ID + 1;
          id = ID;
        end
    end
end

